{
  "cells": [
    {
      "cell_type": "markdown",
      "id": "7198627a",
      "metadata": {},
      "source": [
        "# Gal3ImuEKF\n",
        "\n",
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/navigation/doc/Gal3ImuEKF.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>\n",
        "\n",
        "## Overview\n",
        "\n",
        "The `Gal3ImuEKF` is a left-invariant Lie group EKF for the `Gal3` group, which represents a kinematic state `(R, p, v, t)`. It integrates IMU measurements to predict motion and supports generic measurement updates. This notebook provides a guide to its usage, covering:\n",
        "\n",
        "- The `Gal3` state and the EKF's modes of operation.\n",
        "- How to initialize and use the EKF.\n",
        "- The IMU prediction step and covariance propagation.\n",
        "- How to perform measurement updates.\n",
        "\n",
        "For background on the `Gal3` group itself, see the [Gal3 documentation](https://gtsam.org/doxygen/4.2.0/a03228.html) and the associated [Jupyter notebook](../geometry/doc/Gal3.ipynb)."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "id": "07ea76b3",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "# Install GTSAM and Plotly from pip if running in Google Colab\n",
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop \n",
        "except ImportError:\n",
        "    pass # Not in Colab"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "id": "ec3db055",
      "metadata": {},
      "outputs": [],
      "source": [
        "import numpy as np\n",
        "\n",
        "import gtsam\n",
        "from gtsam import Gal3, Rot3, Pose3"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "cd7ee75f",
      "metadata": {},
      "source": [
        "## The Gal3 State and EKF Modes\n",
        "\n",
        "A `Gal3` state `X = (R, p, v, t)` contains:\n",
        "- `R`: attitude (rotation) in SO(3)\n",
        "- `p`: position in R³\n",
        "- `v`: velocity in R³\n",
        "- `t`: time in R\n",
        "\n",
        "The `Gal3ImuEKF` has three modes of operation, defined by the `Gal3ImuEKF.Mode` enum:\n",
        "1.  `NO_TIME`: The EKF state remains in the `NavState` subgroup, and time is not tracked (`t` is always 0). This is analogous to a standard `NavState`-based filter.\n",
        "2.  `TRACK_TIME_NO_COVARIANCE`: Time is tracked (`t` increases with `dt`), but its uncertainty is not included in the covariance matrix. This is the default mode.\n",
        "3.  `TRACK_TIME_WITH_COVARIANCE`: Time is tracked, and its variance is included in the 10x10 covariance matrix. This makes the filter non-invariant, as the state transition depends on the current time `t_k`.\n",
        "\n",
        "The tangent space (local coordinates) for `Gal3` is a 10D vector `[δθ, δv, δp, δt]`."
      ]
    },
    {
      "cell_type": "markdown",
      "id": "17673a29",
      "metadata": {},
      "source": [
        "## API Overview\n",
        "\n",
        "Class: `gtsam.Gal3ImuEKF(X0, P0, params, mode)`\n",
        "- Constructor: Takes an initial state `X0: Gal3`, covariance `P0: 9x9 or 10x10`, `PreintegrationParams`, and an optional `mode`.\n",
        "- Accessors: `state() -> Gal3`, `covariance() -> numpy array`.\n",
        "- Predict: `predict(omega, accel, dt)` integrates IMU measurements over `dt` and propagates the covariance.\n",
        "- Update: Inherits from `InvariantEKF`, providing methods like `update(measurement, H, R)` for measurement updates."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "id": "fd2c0dee",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Initialized EKF: R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 0 0 0\n",
            "v: 0 0 0\n",
            "t: 0\n",
            "\n",
            "Initial covariance shape: (10, 10)\n",
            "\n",
            "Initialized 10D EKF: R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 0 0 0\n",
            "v: 0 0 0\n",
            "t: 0\n",
            "\n",
            "Initial 10D covariance shape: (10, 10)\n"
          ]
        }
      ],
      "source": [
        "# EKF parameters: gravity and IMU noise\n",
        "params = gtsam.PreintegrationParams.MakeSharedD(9.81)  # gravity (m/s^2)\n",
        "params.setAccelerometerCovariance(np.diag([1e-3, 1e-3, 1e-3]))\n",
        "params.setGyroscopeCovariance(np.diag([1e-4, 1e-4, 1e-4]))\n",
        "# Note: Gal3ImuEKF also uses integrationCovariance for the IMU velocity integral\n",
        "params.setIntegrationCovariance(np.diag([1e-5, 1e-5, 1e-5]))\n",
        "\n",
        "\n",
        "# Initial state and covariance\n",
        "X0 = Gal3() # Identity Gal3 state (R=I, p=0, v=0, t=0)\n",
        "P0 : np.ndarray = np.eye(9) * 0.1 # Initial covariance for (R, v, p)\n",
        "\n",
        "# Create EKF in default mode (TRACK_TIME_NO_COVARIANCE)\n",
        "# The covariance matrix will be 9x9\n",
        "ekf = gtsam.Gal3ImuEKF(X0, P0, params)\n",
        "print(\"Initialized EKF:\", ekf.state())\n",
        "print(\"Initial covariance shape:\", ekf.covariance().shape)\n",
        "\n",
        "# Create EKF that tracks time covariance\n",
        "P0 = np.eye(10) * 0.1\n",
        "ekf = gtsam.Gal3ImuEKF(X0, P0, params, gtsam.Gal3ImuEKF.Mode.TRACK_TIME_WITH_COVARIANCE)\n",
        "print(\"\\nInitialized 10D EKF:\", ekf.state())\n",
        "print(\"Initial 10D covariance shape:\", ekf.covariance().shape)"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c2da3e72",
      "metadata": {},
      "source": [
        "## Predict: Integrate IMU\n",
        "\n",
        "The `predict(omega, accel, dt)` method advances the state and propagates covariance. The dynamics are modeled as `X_{k+1} = W * X_k * U`, where `W` is a left-multipled gravity term and `U` is a right-multiplied IMU increment.\n",
        "\n",
        "- `omega`: body angular velocity (rad/s).\n",
        "- `accel`: specific force (m/s²), in the body frame.\n",
        "- `dt`: timestep (s).\n",
        "\n",
        "The IMU increment `U` is calculated by `Gal3.Expmap` of the IMU measurements, providing an exact integration for piecewise-constant measurements. The gravity term `W` depends on the chosen `mode` to handle time correctly."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "18cc84d3",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Default EKF (9x9 covariance):\n",
            "After predict, state: R: [\n",
            "\t1, 0.001, 0;\n",
            "\t-0.001, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 0 0 0\n",
            "v: 0 0 0\n",
            "t: 0.01\n",
            "\n",
            "Covariance diag: [0.31622935 0.31622935 0.31622935 0.31776148 0.31776148 0.31624358\n",
            " 0.31624377 0.31624377 0.31624374 0.31622777]\n",
            "\n",
            "10D EKF (tracks time covariance):\n",
            "After predict, state: R: [\n",
            "\t0.999998, 0.002, 0;\n",
            "\t-0.002, 0.999998, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "r: 0 0 0\n",
            "v: 0 0 0\n",
            "t: 0.02\n",
            "\n",
            "Covariance diag: [0.31623093 0.31623093 0.31623093 0.32228784 0.32228784 0.31625939\n",
            " 0.31629193 0.31629193 0.31629132 0.31622777]\n"
          ]
        }
      ],
      "source": [
        "# Example predict with constant yaw rate and gravity-only accel\n",
        "omega_b = np.array([0.0, 0.0, -0.1])  # rad/s\n",
        "f_b = np.array([0.0, 0.0, -9.81])  # m/s^2 (specific force)\n",
        "dt = 0.01  # s\n",
        "\n",
        "print(\"Default EKF (9x9 covariance):\")\n",
        "ekf.predict(omega_b, f_b, dt)\n",
        "print(\"After predict, state:\", ekf.state())\n",
        "print(\"Covariance diag:\", np.sqrt(np.diag(ekf.covariance())))\n",
        "\n",
        "print(\"\\n10D EKF (tracks time covariance):\")\n",
        "ekf.predict(omega_b, f_b, dt)\n",
        "print(\"After predict, state:\", ekf.state())\n",
        "print(\"Covariance diag:\", np.sqrt(np.diag(ekf.covariance())))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "1f9ff4e4",
      "metadata": {},
      "source": [
        "## Update: Incorporating Measurements\n",
        "\n",
        "The `Gal3ImuEKF` inherits its `update` method from `InvariantEKF`. The generic signature is `update(measurement, H, R)`, where `H` is the measurement Jacobian with respect to the EKF's local coordinates.\n",
        "\n",
        "For a world position measurement `z ≈ p_world`:\n",
        "- The error is `z - state.position()`.\n",
        "- The Jacobian `H` maps the 10D tangent vector `[δθ, δv, δp, δt]` to the measurement space. For a position measurement, this is `H = [0, 0, I, 0]`, a 3x10 matrix."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "id": "a2a94ec6",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "After position update, state: R: [\n",
            "\t0.999998, 0.002, -3.26651e-05;\n",
            "\t-0.002, 0.999998, 1.64143e-05;\n",
            "\t3.26979e-05, -1.63489e-05, 1\n",
            "]\n",
            "r:   0.0166556 -0.00836948 3.40718e-07\n",
            "v:  0.000339402 -0.000170551  6.94304e-09\n",
            "t: 0.02\n",
            "\n",
            "Covariance diag: [0.31623083 0.31623083 0.31623093 0.3222771  0.3222771  0.31624885\n",
            " 0.28872394 0.28872394 0.28872348 0.31622777]\n"
          ]
        }
      ],
      "source": [
        "# World-position update example for the 10D EKF\n",
        "X_pred = ekf.state()\n",
        "p_pred = X_pred.position()\n",
        "\n",
        "# Jacobian for a world position measurement (w.r.t. 10D tangent space)\n",
        "H = np.zeros((3, 10))\n",
        "H[:, 6:9] = np.eye(3)\n",
        "\n",
        "# Fake measurement with some noise\n",
        "z = p_pred + np.array([0.1, -0.05, 0.0])\n",
        "R_meas = np.eye(3) * 0.5  # measurement covariance (m^2)\n",
        "\n",
        "# Create a measurement object\n",
        "# The error is computed inside the update as (z - h(X)) = z - X.position()\n",
        "\n",
        "ekf.updateWithVector(p_pred, H, z, R_meas)\n",
        "print(\"After position update, state:\", ekf.state())\n",
        "print(\"Covariance diag:\", np.sqrt(np.diag(ekf.covariance())))"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "55ade85a",
      "metadata": {},
      "source": [
        "## Full examples and plotting\n",
        "\n",
        "For a step-by-step tutorial with scenarios, plots, and uncertainty bands, see:\n",
        "- Gal3 IMU EKF Tutorial: https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/Gal3ImuExample.ipynb"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "c65d5356",
      "metadata": {},
      "source": [
        "## Source Code\n",
        "\n",
        "- [Gal3ImuEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Gal3ImuEKF.h)\n",
        "- [Gal3ImuEKF.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/Gal3ImuEKF.cpp)\n",
        "- [InvariantEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/InvariantEKF.h)\n",
        "- [ManifoldEKF.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ManifoldEKF.h)"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}